#include "fashionstarservo.h"
#include <QtMath>

#include <QDebug>


FashionStarServo::FashionStarServo(QObject *parent, QSerialPort *serial) : QObject(parent)
{
    m_serial = serial;
    m_timer = new QTimer(this);
    connect(m_timer, &QTimer::timeout,this,&FashionStarServo::onTimeout);
    connect(m_serial, &QSerialPort::readyRead, this, &FashionStarServo::onReadSerialData);
}


uint8_t FashionStarServo::checksum(uint8_t* data, uint16_t len)
{
    uint8_t sum=0;

    for (uint16_t i=0; i<len; i++)
    {
        sum += data[i];
    }
    return sum;
}

void FashionStarServo::setAngle(uint8_t id, double angle, uint16_t interval, uint16_t power)
{
    uint8_t pack[16];
    uint16_t index=0;
    int16_t angleValue = static_cast<int16_t>(angle*10);

    pack[index++] = 0x12;
    pack[index++] = 0x4c;
    pack[index++] = 0x08;
    pack[index++] = 0x07;
    pack[index++] = id;
    pack[index++] = (angleValue>>0)&0xff;
    pack[index++] = (angleValue>>8)&0xff;
    pack[index++] = (interval>>0)&0xff;
    pack[index++] = (interval>>8)&0xff;
    pack[index++] = (power>>0)&0xff;
    pack[index++] = (power>>8)&0xff;
    uint8_t sum = checksum(pack,index);
    pack[index++] = sum;

    if (m_serial->isOpen())
    {
        m_serial->write(reinterpret_cast<const char*>(pack), index);
    }
    else
    {
        qDebug() << "serial is not open";
    }
}

void FashionStarServo::onTimeout()
{
    static uint16_t cnt=0;
    double angle,angle2;
    //static uint8_t flag=0;

    if (cnt++==49)
    {
        cnt = 0;
        //flag = !flag;
    }

    angle = qSin(2*3.14159*cnt/50)*135;//flag==0?(135.0):(-135.0);
    angle2 = qCos(2*3.14159*cnt/50)*135;//flag==0?(135.0):(-135.0);

    if (m_serial->isOpen())
    {
        setAngle(0x00, angle, 100, 0);
        setAngle(0x01, angle2, 100, 0);
        setAngle(0x02, angle2, 100, 0);
        setAngle(0x03, angle2, 100, 0);

        for(uint8_t i=4; i<=20; i++)
        {
            setAngle(i, angle2, 100, 0);
        }
    }
    qDebug() << angle;
}

void FashionStarServo::onStartTimer()
{
    m_timer->start(20);
}

void FashionStarServo::onStopTimer()
{
    m_timer->stop();
}

void FashionStarServo::onReadSerialData()
{
    const QByteArray data = m_serial->readAll();
    qDebug() << data;
}

void FashionStarServo::readData(uint8_t *data, uint16_t len)
{
    static uint8_t pack[256];
    static uint8_t status=0;
    static uint8_t packetLength;
    static uint8_t j=0;

    for (uint16_t i=0; i<len; i++)
    {
        switch (status)
        {
        case 0://find header 0x05
            if (data[i]==0x05)
            {
                pack[0] = 0x05;
                status = 1;
            }
            break;
        case 1://find header 0x1c
            if (data[i]==0x1c)
            {
                pack[1] = 0x1c;
                status = 2;
            }
            else
            {
                status = 0;
            }
            break;
        case 2:
            // store packetNum
            pack[2] = data[i];
            status = 3;
            break;
        case 3:
            //packet length
            packetLength = data[i];
            j = 0;
            pack[3] = data[i];
            status = 4;
            break;
        case 4:
            pack[j+4] = data[i];
            if (++j>=packetLength)
            {
                status = 5;
            }
            break;
        case 5:
            if (checksum(pack,packetLength+4)==data[i])
            {
                execPacket(&pack[2]);
            }
            break;
        }
    }
}

void FashionStarServo::execPacket(uint8_t *data)
{
    uint8_t packNum = data[0];
    uint8_t packLen = data[1];


    switch (packNum)
    {
    case Ping:
        if (packLen==1)
        {

        }
        break;
    case ResetUserData:
        if (packLen==2)
        {

        }
        break;
    case ReadData:
    {
        uint8_t id = data[2];
        uint8_t dataId = data[3];

        qDebug() << id << dataId;

        break;
    }
    case WriteData:
        if (packLen==3)
        {

        }
        break;
    case ReadBatchData:
        if (packLen==33)
        {

        }
        break;
    case WriteBatchData:
        if (packLen==2)
        {


        }
        break;
    case MoveOnWheelMode:
        break;
    case MoveOnAngleMode:
        break;
    case MoveOnDampingMode:
        break;
    case ReadAngle:
        if (packLen==3)
        {

        }
        break;
    default:
        break;
    }
}
